#include <stdlib.h>
#include <stdio.h>
#include <iomanip>

#include <cv.h>
#include <cvaux.h>
#include <highgui.h>

#include <fstream>
#include <sstream>
#include <sys/time.h>
#include <ctime>

#include "plotAPI.h"

#define LENGTH 180

#define MAX_WINDOW 250
#define MIN_WINDOW 15

class windowTracking {

public:

    typedef struct {
    	char name[20];
	int x_pos;
	int y_pos;
        double x_heading;
        double y_heading;

	int WINDOW_SIZE;
	int x_WINDOW_SIZE;
	int y_WINDOW_SIZE;
	int MAX_WINDOW_SIZE;
	int MIN_WINDOW_SIZE;
	int state;
	bool collision;

	struct {
	    double spatialStd;
	  double x_std;
	  double y_std;
	    double hueMean;
	    double hueStd;	
	} probability;

    } windowProperties;

    struct {
	double spatialStd;
      double x_std;
      double y_std;
	double hueMean;
	double hueStd;	
    } probability;

    double traces [4][LENGTH];
    int traceCounter;
    int bufferCounter;
    double speedVector [2];
    char folder [200];
    int time_lost;

    FILE *myFile;

    windowProperties position;

    CvFont font;
    CvFont font2;

    CvBox2D box;
    
    windowTracking (char *name, int MAX_WINDOW_SIZE, int MIN_WINDOW_SIZE, int x_tmp, int y_tmp)
	{
	    init_traces (1000);
	    position.MAX_WINDOW_SIZE = MAX_WINDOW_SIZE;
	    position.MIN_WINDOW_SIZE = MIN_WINDOW_SIZE;
	    position.WINDOW_SIZE = position.MIN_WINDOW_SIZE;
	    position.x_WINDOW_SIZE = position.MIN_WINDOW_SIZE;
	    position.y_WINDOW_SIZE = position.MIN_WINDOW_SIZE;
	    position.x_pos = x_tmp;
	    position.y_pos = y_tmp;
	    sprintf (position.name,"%s",name);

	    // Init font
	    cvInitFont(&font,CV_FONT_VECTOR0,0.5,0.5,0.0,1);
	    cvInitFont(&font2,CV_FONT_VECTOR0,0.8,0.8,0.0,2);
	}
    
    // Inits the trace counter and positions
    void init_traces (int variance)
	{
	    for (int i=0;i<LENGTH;i++)
	    {
		traces [0][i] = -1;
		traces [1][i] = -1;
		traces [2][i] = -1;
		speedVector [0] = 0.;
		speedVector [1] = 0.;
		position.x_heading = 0.0;
		position.y_heading = 0.0;
		traceCounter = 0;
		time_lost = 0;
		myFile = NULL;
		position.probability.spatialStd = variance;
		position.probability.x_std = variance;
		position.probability.y_std = variance;
		bufferCounter = 0;
		position.collision = false;
	    }
	}


    /* Gaussian window
                                         2
                1              1 |x - mu|
f(x) = ---------------- exp( - - |------| )
       sqrt(2 pi) sigma        2 |sigma |
    */

    double gaussian (double distance, double std)
	{
	    return (1.0/(2.51*sqrt(std))*exp(-0.5*distance/(std)));
	}

    // Finds and situates the object within the window and gives back the size of it,
    // good candidate for making a window that depends on the size of the object (i.e. always 30% bigger than the object)
    double find (IplImage* segmented)    
	{
	    double x_tmp = 0;
	    double y_tmp = 0;
	    double std_tmp = 0;
	    double x_std = 0;
	    double y_std = 0;
	    
	    double iCounter = 0;
	    int jCounter = 0;
	    double distance = 0;
	    
	    uchar prob;

	    double prob_tmp1;

	    // Contour fitting
	    CvMemStorage* storage = cvCreateMemStorage(0);
	    CvPoint pt0;

	    // Fitting
	    CvSeq* ptseq = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvContour),
					sizeof(CvPoint), storage );

	    for(int iY=position.y_pos-position.y_WINDOW_SIZE/2; (iY < position.y_pos+position.y_WINDOW_SIZE/2); iY++) {
		for(int iX=position.x_pos-position.x_WINDOW_SIZE/2; (iX < position.x_pos+position.x_WINDOW_SIZE/2); iX++){
		  if ((iX >= 0)&&(iY >= 0)&&(iX < segmented->width)&&(iY < segmented->height))
		    {
			prob = ((uchar*) (segmented->imageData))[(iX+iY*segmented->width)];

			if (prob)
			{
			    // Updating Center of mass (taking into account the position prediction
			    distance = pow(iX-(position.x_pos + 5*speedVector [0]),2)+pow(iY-(position.y_pos + 5*speedVector [1]),2);
			    prob_tmp1 = gaussian(distance,position.probability.spatialStd)/ (gaussian(0,position.probability.spatialStd)) * 255+prob;

			    x_tmp += (iX*prob_tmp1);
			    y_tmp += (iY*prob_tmp1);

			    std_tmp += distance*prob_tmp1/255;
			    
			    // Rectangle instead of box
			    x_std += 50*fabs(iX-(position.x_pos + 5*speedVector [0]))*prob_tmp1/255;
			    y_std += 50*fabs(iY-(position.y_pos + 5*speedVector [1]))*prob_tmp1/255;


			    iCounter += prob_tmp1;
			    jCounter ++;

			    // Fitting contour
			    pt0.x = iX;
			    pt0.y = iY;
		
			    if (prob_tmp1 > 275)
			      cvSeqPush( ptseq, &pt0 );

			    //prob = ((uchar *)(segmented->imageData))[(iX+iY*segmented->width)];
			}		
		    }
		}
	    }

	    // Update center of mass and update window size
	    if (iCounter > 0) 
	    {	
		position.probability.spatialStd = 0.15 * std_tmp/jCounter + 0.85 * position.probability.spatialStd;
		position.probability.x_std = 0.15 * x_std/jCounter + 0.85 * position.probability.x_std;
		position.probability.y_std = 0.15 * y_std/jCounter + 0.85 * position.probability.y_std;

		int x = (int) (x_tmp/iCounter-position.x_pos);
		int y = (int) (y_tmp/iCounter-position.y_pos);
			
		//Compute heading direction
		if (sqrt(speedVector[0]*speedVector[0]+speedVector[1]*speedVector[1]) > 0)
		  {
		    double x_heading = (double)speedVector[0]/sqrt(speedVector[0]*speedVector[0]+speedVector[1]*speedVector[1]);
		    double y_heading = (double)speedVector[1]/sqrt(speedVector[0]*speedVector[0]+speedVector[1]*speedVector[1]);

		    position.x_heading = 0.8*position.x_heading+ 0.2*x_heading;
		    position.y_heading = 0.8*position.y_heading+ 0.2*y_heading;
		  }
		

		// Draw heading direction
		//draw_arrow (CV_RGB(255,255,255),(int) 5*position.x_heading,(int) 5*position.y_heading,segmented,position.x_pos,position.y_pos);

		int jump = x*x +y*y;
		if (jump > 0)
		  //min Pixel jump to move the window
		{
		  speedVector [0] = x*0.15 + speedVector [0]*0.85;
		  speedVector [1] = y*0.15 + speedVector [1]*0.85;
		  position.x_pos = (int) (x_tmp/iCounter);
		  position.y_pos = (int) (y_tmp/iCounter); 
		}
		
		// For x and y
		if (position.probability.x_std > 0.02*position.x_WINDOW_SIZE*position.x_WINDOW_SIZE)
		    if (position.x_WINDOW_SIZE < position.MAX_WINDOW_SIZE)	
			position.x_WINDOW_SIZE += 12;
		if (position.x_WINDOW_SIZE*position.x_WINDOW_SIZE/40.0 > position.probability.x_std)
		    if (position.x_WINDOW_SIZE > position.MIN_WINDOW_SIZE)
			position.x_WINDOW_SIZE -= 8;
		if (position.probability.y_std > 0.02*position.y_WINDOW_SIZE*position.y_WINDOW_SIZE)
		    if (position.y_WINDOW_SIZE < position.MAX_WINDOW_SIZE)	
			position.y_WINDOW_SIZE += 12;
		if (position.y_WINDOW_SIZE*position.y_WINDOW_SIZE/40.0 > position.probability.y_std)
		    if (position.y_WINDOW_SIZE > position.MIN_WINDOW_SIZE)
			position.y_WINDOW_SIZE -= 8;

	
		position.state = 0;			//Found
	    }
	    else  // Increase search area if lost and predict movement
	    {
		position.probability.spatialStd += 0.4;
		position.probability.x_std += 0.4;
		position.probability.y_std += 0.4;

		if (position.x_WINDOW_SIZE < position.MAX_WINDOW_SIZE)
		  position.x_WINDOW_SIZE +=12;
		
		if (position.y_WINDOW_SIZE < position.MAX_WINDOW_SIZE)
		  position.y_WINDOW_SIZE +=12;
		    
		/*cvPutText (segmented,"?"				\
		  ,cvPoint(position.x_pos+position.WINDOW_SIZE/2-19,position.y_pos+position.WINDOW_SIZE/2-11), &font2,CV_RGB(255,255,255));*/
		
		position.state = 1;			//Lost
		    
		    // keep inertia if lost
		    //position.x_pos += (int) speedVector [0];
		    //position.y_pos += (int) speedVector [1];
		    //speedVector [0] *= 0.6;
		    //speedVector [1] *= 0.6;
		
	    }
	    //draw_cross (CV_RGB(255,255,255), 5, segmented, position.x_pos, position.y_pos);
	    //draw_square (CV_RGB(255,255,255), position.WINDOW_SIZE, segmented, position.x_pos, position.y_pos);
	    //draw_rectangle (CV_RGB(255,255,255), position.x_WINDOW_SIZE/2,position.y_WINDOW_SIZE/2, segmented, position.x_pos, position.y_pos);
	    /*cvPutText (segmented,position.name			\
	      ,cvPoint (position.x_pos-position.x_WINDOW_SIZE/2+1,position.y_pos-position.y_WINDOW_SIZE/2-1),&font,CV_RGB(255,255,255));*/

	    // Contour fitting
	    if (ptseq->total > 0 )
	      {
		box = cvMinAreaRect2( ptseq, 0 );

		if (box.size.width < box.size.height)
		  {
		    box.angle = box.angle;
		  }
		else
		  {
		    box.angle = 90+box.angle-180;
		    int tmp = box.size.width;
		    box.size.width = box.size.height;
		    box.size.height = tmp;
		  }
		
	      }
	    return (jCounter);
	}
  
  void getPosition (int* x, int* y)
  {
    *x = position.x_pos;
    *y = position.y_pos;
  }

  CvBox2D getBox()
  {
    return (box);
  }

 //  int getOrientation()
//   {
//     int angle;

//     if (box.size.width < box.size.height)
//       angle = -box.angle;
//     else
//       angle = 90-box.angle;

//     return (angle);
//   }

};
